import pandas as pd
import numpy as np


# 打印 col 基础信息
def print_col_first_max_min_mean(col, col_name):
    print(f"\n{col_name}:")
    print(f"  第一个: {col[0]}")
    print(f"  最大值: {col.max()}")
    print(f"  最小值: {col.min()}")
    print(f"  均值: {col.mean()}\n")


# 读取并转置 csv
def read_csv_transpose(path_csv):
    # 1. 读取 CSV 文件
    df = pd.read_csv(path_csv, header=None)
    # 2. 转置 DataFrame
    df_transposed = df.T
    return df_transposed


# xy坐标转经纬度
def xy_2_longlati(col_x, col_y, longitude_end, latitude_start):
    """
    将x, y坐标列表转换为经纬度。

    :param col_x: x坐标的列表 (pandas Series 或 list-like)
    :param col_y: y坐标的列表 (pandas Series 或 list-like)
    :param longitude_end: 起始点 (col_x[0], col_y[0]) 对应的经度
    :param latitude_start: 起始点 (col_x[0], col_y[0]) 对应的纬度
    :return: (longitudes, latitudes) 两个列表，分别是转换后的经度和纬度
    """
    # 将输入转换为 numpy 数组以便计算
    x_array = np.array(col_x)
    y_array = np.array(col_y)

    # 计算起始点的 x 和 y 坐标
    start_x = x_array[0]
    start_y = y_array[0]

    # 计算 x 和 y 方向的差值
    dx = x_array - start_x
    dy = y_array - start_y

    # 地球平均半径 (米)
    earth_radius = 6371000.0

    # 计算经度变化 (弧度)
    delta_longitude_rad = dx / (earth_radius * np.cos(np.radians(latitude_start)))

    # 计算纬度变化 (弧度)
    delta_latitude_rad = dy / earth_radius

    # 转换回角度并计算最终经纬度
    longitudes = longitude_end + np.degrees(delta_longitude_rad)
    latitudes = latitude_start + np.degrees(delta_latitude_rad)

    # 返回列表格式
    # return longitudes.tolist(), latitudes.tolist()
    return longitudes, latitudes


################################################ 对齐 #############################################
def lonlat_adjust(df_dataset, lon_smooth, lat_smooth):
    # 对齐 lon_smooth 和 lat_smooth 的第一个值
    # 获取原始真实数据的第一个值
    first_lon_real = df_dataset['lon_smooth'].iloc[0]
    first_lat_real = df_dataset['lat_smooth'].iloc[0]
    # 计算当前恢复序列第一个值与真实值之间的偏差
    delta_lon = first_lon_real - lon_smooth[0]
    delta_lat = first_lat_real - lat_smooth[0]
    # 对整个序列加上偏差（整体平移），实现对齐
    lon_smooth = lon_smooth + delta_lon
    lat_smooth = lat_smooth + delta_lat
    return lon_smooth, lat_smooth


################################################ 主函数 #############################################
def format_matlab_caekf(path_csv_dataset, path_csv_xy, path_csv_coord, longitude_start, latitude_start):
    # 1. 读取 csv 并获取 x 和 y
    df_transposed = read_csv_transpose(path_csv_xy)
    col_x = df_transposed.iloc[:, 0]    # 第一列: x
    col_y = df_transposed.iloc[:, 1]    # 第二列: y
    print_col_first_max_min_mean(col_x, "col_x")
    print_col_first_max_min_mean(col_y, "col_y")
    # 2. 将 x 和 y 转化为经纬度
    longitudes_ori, latitudes_ori = xy_2_longlati(col_x, col_y, longitude_start, latitude_start)
    print_col_first_max_min_mean(longitudes_ori, "longitudes_ori")
    print_col_first_max_min_mean(latitudes_ori, "latitudes_ori")
    # 3. 对齐 longitudes 和 latitudes 的第一个值
    df = pd.read_csv(path_csv_dataset)
    longitudes, latitudes = lonlat_adjust(df, longitudes_ori, latitudes_ori)
    # 打印结果示例
    print("转换后的经纬度坐标:")
    for i in range(min(5, len(longitudes))):  # 只打印前5个作为示例
        print(f"Point {i}: Longitude = {longitudes[i]:.8f}, Latitude = {latitudes[i]:.8f}")
    print("----------------------------------------------------------------")
    # 3. 保存到 CSV 文件
    result_df = pd.DataFrame({
        'Longitude_ori': longitudes_ori,
        'Latitude_ori': latitudes_ori,
        'Longitude': longitudes,
        'Latitude': latitudes,
        'kf_col_x': col_x,
        'kf_col_y': col_y
    })
    result_df.to_csv(path_csv_coord, index=False)
    print(f"\n结果已保存到 {path_csv_coord}")


def format_matlab_immukf(path_csv_dataset, path_csv_xy, path_csv_coord, longitude_start, latitude_start):
    # 1. 读取 csv 并获取 x 和 y
    df_transposed = read_csv_transpose(path_csv_xy)
    raw_col_x_str = df_transposed.iloc[:, 0]    # 第一列: x
    raw_col_y_str = df_transposed.iloc[:, 1]    # 第二列: y
    # 使用 pandas 的 apply 方法对每行进行处理
    col_x = raw_col_x_str.apply(lambda x: complex(x.replace('i', 'j'))).apply(lambda c: c.real)
    # 处理 y 列：同样转换为复数，然后提取实部
    col_y = raw_col_y_str.apply(lambda x: complex(x.replace('i', 'j'))).apply(lambda c: c.real)
    # 打印数据
    print_col_first_max_min_mean(col_x, "col_x")
    print_col_first_max_min_mean(col_y, "col_y")
    # 2. 将 x 和 y 转化为经纬度
    longitudes_ori, latitudes_ori = xy_2_longlati(col_x, col_y, longitude_start, latitude_start)
    print_col_first_max_min_mean(longitudes_ori, "longitudes_ori")
    print_col_first_max_min_mean(latitudes_ori, "latitudes_ori")
    # 3. 对齐经纬度
    df = pd.read_csv(path_csv_dataset)
    longitudes, latitudes = lonlat_adjust(df, longitudes_ori, latitudes_ori)
    # 打印结果示例
    print("转换后的经纬度坐标:")
    for i in range(min(5, len(longitudes))):  # 只打印前5个作为示例
        print(f"Point {i}: Longitude = {longitudes[i]:.8f}, Latitude = {latitudes[i]:.8f}")
    print("----------------------------------------------------------------")
    # 3. 保存到 CSV 文件
    result_df = pd.DataFrame({
        'Longitude_ori': longitudes_ori,
        'Latitude_ori': latitudes_ori,
        'Longitude': longitudes,
        'Latitude': latitudes,
        'kf_col_x': col_x,
        'kf_col_y': col_y
    })
    result_df.to_csv(path_csv_coord, index=False)
    print(f"\n结果已保存到 {path_csv_coord}")



if __name__=="__main__":
    # # measure3: dataset前3000
    # longitude_start = -122.42152728959445
    # latitude_start = 37.77239080237695
    # measure4: dataset同AE-LSTM方法
    longitude_start = -122.416739638954
    latitude_start = 37.7723101949436
    format_matlab_caekf('uav_dataset_thph_xy_lonlat_dataset_test.csv',
                        'caekf_out_Xe_Measure4.csv', 'caekf_out_Xe_Measure4_2_coord_2.csv',
                        longitude_start, latitude_start)
    format_matlab_immukf('uav_dataset_thph_xy_lonlat_dataset_test.csv',
                         'immukf_out_Xe_Measure4.csv', 'immukf_out_Xe_Measure4_2_coord_2.csv',
                         longitude_start, latitude_start)